
#include "mb_tcp_slave.h"
/*
 * Copyright © Stéphane Raimbault <stephane.raimbault@gmail.com>
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <errno.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <pthread.h>

#define NB_CONNECTION (10)

static modbus_t *mb_ctx = NULL;
static modbus_mapping_t *mb_mapping;
static pthread_mutex_t mb_mutex;
static int server_socket = -1;
pthread_t mb_thread;

void *modbus_tcp_work(void *msg)
{
    dev_cfg_t *cfg = (dev_cfg_t *)msg;
    uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH];
    int master_socket;
    int rc;
    fd_set refset;
    fd_set rdset;
    /* Maximum file descriptor number */
    int fdmax;

    mb_ctx = modbus_new_tcp(cfg->modbus.mb_ip, cfg->modbus.mb_port);
    xx_log_info("modbus tcp ip:'%s', port: %d", cfg->modbus.mb_ip, cfg->modbus.mb_port);
    server_socket = modbus_tcp_listen(mb_ctx, NB_CONNECTION);
    if (server_socket == -1)
    {
        xx_log_error("Unable to listen TCP connection");
        modbus_free(mb_ctx);
        return NULL;
    }

    /* Clear the reference set of socket */
    FD_ZERO(&refset);
    /* Add the server socket */
    FD_SET(server_socket, &refset);

    /* Keep track of the max file descriptor */
    fdmax = server_socket;

    for (;;)
    {
        rdset = refset;
        if (select(fdmax + 1, &rdset, NULL, NULL, NULL) == -1)
        {
            xx_log_error("Server select() failure.");
            mb_slave_deinit();
        }

        /* Run through the existing connections looking for data to be
         * read */
        for (master_socket = 0; master_socket <= fdmax; master_socket++)
        {

            if (!FD_ISSET(master_socket, &rdset))
            {
                continue;
            }

            if (master_socket == server_socket)
            {
                /* A client is asking a new connection */
                socklen_t addrlen;
                struct sockaddr_in clientaddr;
                int newfd;

                /* Handle new connections */
                addrlen = sizeof(clientaddr);
                memset(&clientaddr, 0, sizeof(clientaddr));
                newfd = accept(server_socket, (struct sockaddr *)&clientaddr, &addrlen);
                if (newfd == -1)
                {
                    xx_log_error("Server accept() error");
                }
                else
                {
                    FD_SET(newfd, &refset);

                    if (newfd > fdmax)
                    {
                        /* Keep track of the maximum */
                        fdmax = newfd;
                    }
                    xx_log_info("New connection from %s:%d on socket %d",
                                inet_ntoa(clientaddr.sin_addr),
                                clientaddr.sin_port,
                                newfd);
                }
            }
            else
            {
                modbus_set_socket(mb_ctx, master_socket);
                rc = modbus_receive(mb_ctx, query);
                if (rc > 0)
                {
                    pthread_mutex_lock(&mb_mutex);
                    modbus_reply(mb_ctx, query, rc, mb_mapping);
                    pthread_mutex_unlock(&mb_mutex);
                }
                else if (rc == -1)
                {
                    /* This example server in ended on connection closing or
                     * any errors. */
                    xx_log_info("Connection closed on socket %d", master_socket);
                    close(master_socket);

                    /* Remove from reference set */
                    FD_CLR(master_socket, &refset);

                    if (master_socket == fdmax)
                    {
                        fdmax--;
                    }
                }
            }
        }
    }
    return NULL;
}

int mb_slave_init(dev_cfg_t *cfg)
{
    mb_mapping =
        modbus_mapping_new(MODBUS_MAX_READ_BITS, 0, MODBUS_MAX_READ_REGISTERS * 20, 0);
    if (mb_mapping == NULL)
    {
        xx_log_error("Failed to allocate the mapping: %s", modbus_strerror(errno));
        modbus_free(mb_ctx);
        return -1;
    }
    pthread_mutex_init(&mb_mutex, NULL);
    int ret = pthread_create(&mb_thread, NULL, modbus_tcp_work, (void *)cfg);
    if (ret != 0)
    {
        xx_log_error("pthread_create returned error code %d", ret);
        return -1;
    }
    return 0;
}

void mb_write(uint16_t *buf, int addr, int len)
{
    pthread_mutex_lock(&mb_mutex);
    uint16_t *p = mb_mapping->tab_registers + addr;
    memcpy(p, buf, len * sizeof(uint16_t));
    pthread_mutex_unlock(&mb_mutex);
}

void mb_read(int addr, int len, uint16_t *buf)
{
    pthread_mutex_lock(&mb_mutex);
    if (addr + len > mb_mapping->nb_registers)
    {
        xx_log_error("read addr not exits");
        goto end;
    }
    uint16_t *p = mb_mapping->tab_registers + addr;
    memcpy(buf, p, len * 2);
end:
    pthread_mutex_unlock(&mb_mutex);
}

void mb_slave_deinit(void)
{
    if (server_socket != -1)
    {
        close(server_socket);
        server_socket = -1;
    }

    if (mb_ctx)
    {
        modbus_free(mb_ctx);
        mb_ctx = NULL;
    }

    if (mb_mapping)
    {
        modbus_mapping_free(mb_mapping);
        mb_mapping = NULL;
    }
    pthread_mutex_destroy(&mb_mutex);
}
